/*
 * @Descripttion: 
 * @version: 
 * @Author: Eugene
 * @Date: 2024-06-29 17:49:43
 * @LastEditors: Andy
 * @LastEditTime: 2024-07-05 15:25:42
 */
#include "operate.h"

arm_matrix_instance_f32 R_Target;
arm_matrix_instance_f32 R_Actual;
arm_matrix_instance_f32 R_Error;

arm_matrix_instance_f32 R_Target_Transpose;

void Matrixs_Init(void)
{
    arm_mat_init_f32(&R_Target,3,3,Matrix_Target.Matrix_to_R);
    arm_mat_init_f32(&R_Actual,3,3,Matrix_Actual.Matrix_to_R);
    arm_mat_init_f32(&R_Error,3,3,Matrix_Error.Matrix_to_R);

    arm_mat_init_f32(&R_Target_Transpose,3,3,Matrix_Target_Transpose.Matrix_to_R);
}

void Operate_Task(void)
{
#ifdef R_Matrix_d
    Enler_to_R_Matrix(&EulerAngle_Target , &Matrix_Target);//目标值转化为旋转矩阵

    Quaternion_to_R_Matrix(&Q_info_Actual , &Matrix_Actual);//实际值转化为旋转矩阵

    arm_mat_trans_f32(&R_Target , &R_Target_Transpose);//目标值矩阵转置

    arm_mat_mult_f32(&R_Actual , &R_Target_Transpose , &R_Error);//实际矩阵 右乘 目标转置矩阵 等于 旋转误差矩阵

    R_Matrix_to_Axial(&Matrix_Error , &Axial_Err);//旋转误差矩阵转化为轴角

    Anonymity_TX((int16_t)(Axial_Err.long_m*100*Axial_Err.Angle_0) ,
                 (int16_t)EulerAngle_Target.Pitch*(-5),
                 (int16_t)EulerAngle_Actual.Pitch,
                 0,
                 0,
                 0,
                 0,
                 0,
                 0,
                 0);
#endif

#ifdef Q_info_d

    IMU_Euler_To_Quaternion(&EulerAngle_Target , &Q_info_Target);//目标值转化为四元数

    quaternion_inverse(&Q_info_Actual , &Q_info_Target_instance);//四元数求逆

    quaternion_multiply(&Q_info_Target , &Q_info_Target_instance , &Q_info_Error);//求误差四元数

    Q_info_Error_Linear.q1 = asinf(Q_info_Error.q1);//误差线性化
    Q_info_Error_Linear.q2 = asinf(Q_info_Error.q2);//误差线性化
    Q_info_Error_Linear.q3 = asinf(Q_info_Error.q3);//误差线性化

    Anonymity_TX((int16_t)(Q_info_Error_Linear.q1*1000),
                 (int16_t)(Q_info_Error_Linear.q2*1000),
                 (int16_t)(Q_info_Error_Linear.q3*1000),
                 (int16_t)EulerAngle_Target.Pitch*8.8f,
                 (int16_t)0,
                 (int16_t)0,
                 (int16_t)0,
                 (int16_t)0,
                 (int16_t)0,
                 (int16_t)0);

#endif


}

